#include <motion_controller/controller_ros_node.h>


ControllerRosNode::ControllerRosNode(tf2_ros::Buffer* tf_buffer):
    robotModelPtr_(nullptr),mapHandlerPtr_(nullptr),tfBuffer_(tf_buffer){
    // 机器人模型初始化
    robotModelConfigFile_ = ros::package::getPath("motion_controller") + "/config/robot_model.yaml";
    robotModelPtr_ = boost::make_shared<ModelBase>(robotModelConfigFile_);

    // 初始化
    mapHandlerPtr_ = boost::make_shared<MapBase>();
    int global_planner_type = ros::param::param<int>("/global_planner/planner_type", 0);
    switch(global_planner_type){
        case 0:
            LOG(INFO) << "Path planner is AstarPlanner.";
            globalPathPlannerPtr_ = boost::make_shared<AstarPlanner>(mapHandlerPtr_);
            break;
        case 1:
            LOG(INFO) << "Path planner is JPSPlanner.";
            globalPathPlannerPtr_ = boost::make_shared<JPSPlanner>(mapHandlerPtr_);
            break;
        default:
            globalPathPlannerPtr_ = boost::make_shared<AstarPlanner>(mapHandlerPtr_);
            break;
    }
    int local_controller_type = ros::param::param<int>("/local_controller/controller_type", 0);
    switch(local_controller_type){
        case 0:
            LOG(INFO) << "Controller is DWAController.";
            localControllerPtr = boost::make_shared<DWAController>();
            break;
        case 1:
            LOG(INFO) << "Controller is PurePursuit.";
            localControllerPtr = boost::make_shared<PurePursuit>();
            break;
        case 2:
            LOG(INFO) << "Controller is Stanley.";
            localControllerPtr = boost::make_shared<Stanley>();
            break;
        default:
            localControllerPtr = boost::make_shared<DWAController>();
            break;
    }  


    bevMap_.Init(6.0, 6.0, 0.02, "base_link");
    managerPtr_ = boost::make_shared<ProcessorP2P>(globalPathPlannerPtr_,
        localControllerPtr);

    // ros 
    mapBevPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/map_bev", 1);
    mapDilatePub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/map_dilate", 1, true);
    trackedPathPub_ = nh_.advertise<nav_msgs::Path>("/tracked_path", 1);
    robotModelMarkerPub_ = nh_.advertise<visualization_msgs::MarkerArray>("/robot_model", 1);

    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &ControllerRosNode::RvizPosesCallback, this);
    mapSub_ = nh_.subscribe("/map", 10, &ControllerRosNode::MapCallback, this);
    fusionDataSub_ = nh_.subscribe("/fusion_cloud", 10, &ControllerRosNode::FusionDataCallback, this);

    // 循环控制
    Loop();
}


ControllerRosNode::~ControllerRosNode(){

}

 // 地图回调函数
void ControllerRosNode::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    LOG(INFO) << "Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution;
    occupancyGrid_ = msg;

    // 路径规划器设置地图
    mapHandlerPtr_->setOccupancyGrid(msg);
    // 简单的地图膨胀 (膨胀半径为0.25)
    mapHandlerPtr_->SimpleDilateOccupancyGrid(0.25);

    // 发布膨胀后的地图
    auto map_ptr = boost::make_shared<nav_msgs::OccupancyGrid>();
    if(mapHandlerPtr_->getMap(map_ptr, msg->header.frame_id)){
        mapDilatePub_.publish(*map_ptr);
    }
}

// Rviz位姿回调函数
void ControllerRosNode::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    goalPosePtr_ = msg;
    ROS_INFO("[RvizPosesCallback] get pose, x=%0.3f, y=%0.3f, yaw=%0.3f.", 
        msg->pose.position.x, 
        msg->pose.position.y,
        tf2::getYaw(msg->pose.orientation)
    );

    // 开始进入P2P任务
    managerPtr_->setGoal(*msg);
    managerPtr_->setEvent(StateMachine::Event::EV_WORKING);    
}

// 融合数据回调函数
void ControllerRosNode::FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg){
    fusionDataPtr_ = msg;

    // localControllerPtr->setPointCloudData(msg);

    // // 速度规划器设置地图
    // bevMap_.setPointCloudData(msg);
    // bevMap_.SimpleDilateOpencvMap(0.25);
    // auto map_ptr = boost::make_shared<nav_msgs::OccupancyGrid>();
    // if(bevMap_.getBevMap(map_ptr, msg->header.frame_id)){
    //     mapBevPub_.publish(*map_ptr);
    // }
}


// 循环控制
void ControllerRosNode::Loop(){
    managerPtr_->setEvent(StateMachine::Event::EV_IDLE);
    ros::Rate controller_rate(20);  //20Hz
    while(!ros::isShuttingDown()){
        controller_rate.sleep();
        ros::spinOnce();
        LOG(INFO) << "--------------------------------";

        // 阻塞状态检测
        // 获取机器人当前位姿
        geometry_msgs::PoseStamped global_robot_pose;
        if(!localControllerPtr->getRobotPose(tfBuffer_, global_robot_pose)){
            LOG(WARNING) << "can not get robot pose!";
            // 进入阻塞状态  待补全
            continue;
        }

        // 发布运动轨迹
        trackedPath_.addPoseStamped(global_robot_pose);
        trackedPathPub_.publish(trackedPath_.toPath("map"));

        // 发布机器人模型
        robotModelMarkerPub_.publish(robotModelPtr_->getRobotModelMarkerArray("base_link"));

        // 状态机控制
        ROS_INFO_STREAM("curr_state is " << managerPtr_->GetCurrStateName());
        cout << "global_robot_pose: " << global_robot_pose.pose.position.x << "  " 
            << global_robot_pose.pose.position.y << "  " 
            << tf2::getYaw(global_robot_pose.pose.orientation) << endl;
        managerPtr_->HandleEvent(global_robot_pose);
    }
}


int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    // FLAGS_minloglevel = google::INFO; 
    FLAGS_minloglevel = google::WARNING;    // 只记录大于{}等级的日志
    FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);
    google::InstallFailureSignalHandler();  // 捕获系统信号

    ros::init(argc, argv, "controller_ros_node");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("controller_ros_node start...");

    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    ControllerRosNode controller_ros_node(&tfBuffer);
    ros::spin();

    google::ShutdownGoogleLogging();

    return 0;
}